#pragma once
#include "g2o/core/base_vertex.h"

namespace g2o
{
    class VertexP : public g2o::BaseVertex<3, Eigen::Vector3d>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        VertexP();
        virtual bool read(std::istream &is);

        virtual bool write(std::ostream &os) const;

        virtual void setToOriginImpl() { _estimate.fill(0.); }

        virtual void oplusImpl(const double *update_)
        {
            Eigen::Map<const Eigen::Vector3d> update(update_);
            _estimate += update;
        }

        virtual bool setEstimateDataImpl(const double *est)
        {
            Eigen::Map<const Eigen::Vector3d> _est(est);
            _estimate = _est;
            return true;
        }

        virtual bool getEstimateData(double *est) const
        {
            Eigen::Map<Eigen::Vector3d> _est(est);
            _est = _estimate;
            return true;
        }

        virtual int estimateDimension() const
        {
            return 3;
        }

        virtual bool setMinimalEstimateDataImpl(const double *est)
        {
            _estimate = Eigen::Map<const Eigen::Vector3d>(est);
            return true;
        }

        virtual bool getMinimalEstimateData(double *est) const
        {
            Eigen::Map<Eigen::Vector3d> v(est);
            v = _estimate;
            return true;
        }

        virtual int minimalEstimateDimension() const
        {
            return 3;
        }
    };
} // namespace g2o